#include<costmap_2d/costmap_layer.h>

namespace costmap_2d
{

void CostmapLayer::touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y)
{
    *min_x = std::min(x, *min_x);
    *min_y = std::min(y, *min_y);
    *max_x = std::max(x, *max_x);
    *max_y = std::max(y, *max_y);
}
// 用主地图的尺寸来设置该层地图的尺寸
void CostmapLayer::matchSize()
{
    Costmap2D *master = layered_costmap_->getCostmap();
    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
              master->getOriginX(), master->getOriginY());
}

void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
{
    unsigned char *grid = getCharMap();
    for (int x = 0; x < (int) getSizeInCellsX(); x++)
    {
        bool xrange = x > start_x && x < end_x;
        for (int y = 0; y < (int) getSizeInCellsY(); y++)
        {
            if ((xrange && y > start_y && y < end_y) != invert_area) continue;
            int index = getIndex(x, y);
            // 将栅格更新为未知信息
#ifdef SEMANTIC
            if (toXJUcost(grid[index]) != XJU_COST_NO_INFORMATION)
            {
                grid[index] = toOri(XJU_COST_NO_INFORMATION, XJU_OPTION_INIT);
            }
#else
            if (grid[index] != NO_INFORMATION) grid[index] = NO_INFORMATION;
#endif
        }
    }
}
// 将传入的bound与数据成员的值比较，如果传入的bound范围更大，则更新数据成员的值；
void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
{
    extra_min_x_ = std::min(mx0, extra_min_x_); // 1e6
    extra_max_x_ = std::max(mx1, extra_max_x_); // -1e6
    extra_min_y_ = std::min(my0, extra_min_y_);
    extra_max_y_ = std::max(my1, extra_max_y_);
    has_extra_bounds_ = true;
}

void CostmapLayer::useExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y)
{
    if (!has_extra_bounds_) return;
    *min_x = std::min(extra_min_x_, *min_x);
    *min_y = std::min(extra_min_y_, *min_y);
    *max_x = std::max(extra_max_x_, *max_x);
    *max_y = std::max(extra_max_y_, *max_y);
    extra_min_x_ = 1e6;
    extra_min_y_ = 1e6;
    extra_max_x_ = -1e6;
    extra_max_y_ = -1e6;
    has_extra_bounds_ = false;
}
// 用当前子地图数据（不包括未知cell）更新主地图对应区域.如果主栅格值为NO_INFORMATION,则将其覆盖。如果图层的值为NO_INFORMATION，则主值不会更改。
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
    if (!enabled_) return;
    unsigned char *master_array = master_grid.getCharMap();
    unsigned int span = master_grid.getSizeInCellsX();
    for (int j = min_j; j < max_j; j++)
    {
        unsigned int it = j * span + min_i;
        for (int i = min_i; i < max_i; i++)
        {
            // 如果主栅格值为NO_INFORMATION,则将其覆盖。
#ifdef SEMANTIC
            auto grid_value = toXJUgrid(costmap_[it]);
            if (grid_value.cost == XJU_COST_NO_INFORMATION)
            {
                it++;
                continue;
            }
            auto old_grid_value = toXJUgrid(master_array[it]);
            if (old_grid_value.cost == XJU_COST_NO_INFORMATION || old_grid_value.cost < grid_value.cost)
            {
                setXJUcost(master_array[it], grid_value.cost);
            }
            // 如果图层的值为NO_INFORMATION，则主值不会更改。
            if (grid_value.cost == XJU_COST_LETHAL_OBSTACLE && old_grid_value.option < grid_value.option)
            {
                setXJUoption(master_array[it], grid_value.option);
            }
#else
            if (costmap_[it] == NO_INFORMATION)
            {
                it++;
                continue;
            }
            unsigned char old_cost = master_array[it];
            // 如果图层的值为NO_INFORMATION，则主值不会更改。
            if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
            {
                master_array[it] = costmap_[it];
            }
#endif
            it++;
        }
    }
}
// 用当前子地图数据（包括未知cell）覆盖主地图对应区域
void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D &master_grid,
        int min_i, int min_j, int max_i, int max_j)
{
    if (!enabled_) return;
    unsigned char *master = master_grid.getCharMap();
    unsigned int span = master_grid.getSizeInCellsX();
    for (int j = min_j; j < max_j; j++)
    {
        unsigned int it = span * j + min_i;
        for (int i = min_i; i < max_i; i++)
        {
            master[it] = costmap_[it];
            it++;
        }
    }
}
// 用当前子地图数据（不包括未知cell）覆盖主地图对应区域
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D &master_grid,
        int min_i, int min_j, int max_i, int max_j)
{
    if (!enabled_) return;
    unsigned char *master = master_grid.getCharMap();
    unsigned int span = master_grid.getSizeInCellsX();
    for (int j = min_j; j < max_j; j++)
    {
        unsigned int it = span * j + min_i;
        for (int i = min_i; i < max_i; i++)
        {
            //这里比较特别，只处理非NO_INFORMATION的
#ifdef SEMANTIC
            auto grid_value = toXJUgrid(costmap_[it]);
            if (grid_value.cost != XJU_COST_NO_INFORMATION) master[it] = costmap_[it];
#else
            if (costmap_[it] != NO_INFORMATION) master[it] = costmap_[it];
#endif
            it++;
        }
    }
}

// 用当前子地图数据（不包括未知cell）更新主地图对应区域
// 如果主栅格值是NO_INFORMATION.它被图层的值覆盖。
// 如果图层的值为NO_INFORMATION，则主栅格值不会更改。
// 如果总和值大于INSCRIBED_INFLATED_OBSTACLE,则主值设置为INSCRIBED_INFLATED_OBSTACLE - 1。
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D &master_grid,
        int min_i, int min_j, int max_i, int max_j)
{
    if (!enabled_) return;
    unsigned char *master_array = master_grid.getCharMap();
    unsigned int span = master_grid.getSizeInCellsX();
    for (int j = min_j; j < max_j; j++)
    {
        unsigned int it = j * span + min_i;
        for (int i = min_i; i < max_i; i++)
        {
            // 如果主栅格值是NO_INFORMATION.它被图层的值覆盖。
#ifdef SEMANTIC
            auto grid_value = toXJUgrid(costmap_[it]);
            if (grid_value.cost == XJU_COST_NO_INFORMATION)
            {
                it++;
                continue;
            }
            auto old_grid_value = toXJUgrid(master_array[it]);
            if (old_grid_value.cost == XJU_COST_NO_INFORMATION) master_array[it] = costmap_[it];
            else
            {
                int sum = old_grid_value.cost + grid_value.cost;
                if (sum > XJU_COST_INSCRIBED_INFLATED_OBSTACLE)
                    setXJUcost(master_array[it], XJU_COST_INSCRIBED_INFLATED_OBSTACLE - 1);
                else setXJUcost(master_array[it], sum);
            }
            if (grid_value.cost == XJU_COST_LETHAL_OBSTACLE && old_grid_value.option < grid_value.option)
            {
                setXJUoption(master_array[it], grid_value.option);
            }
#else
            if (costmap_[it] == NO_INFORMATION)
            {
                it++;
                continue;
            }
            unsigned char old_cost = master_array[it];
            // 如果图层的值为NO_INFORMATION，则主栅格值master_array不会更改。
            if (old_cost == NO_INFORMATION) master_array[it] = costmap_[it];
            else
            {
                // 如果总和值大于INSCRIBED_INFLATED_OBSTACLE,则主值master_array设置为INSCRIBED_INFLATED_OBSTACLE - 1。
                int sum = old_cost + costmap_[it];
                if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
                {
                    master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
                }
                else master_array[it] = sum;
            }
#endif
            it++;
        }
    }
}
}  // namespace costmap_2d
